package Main;

import lejos.nxt.*;
import lejos.nxt.addon.OpticalDistanceSensor;


public class Tutka {

   private NXTRegulatedMotor moottori;
   private OpticalDistanceSensor[] sensorit;
   private String[] sensorityypit;
   private int jakajat[];
   
   Tutka(NXTRegulatedMotor moottori, OpticalDistanceSensor[] sensorit) {
       this.moottori = moottori;
       this.sensorit = sensorit;
       this.jakajat = new int[sensorit.length];
       sensorityypit = new String[sensorit.length];

       for (int i = 0; i < sensorit.length; ++i) {
           do {
               sensorityypit[i] = sensorit[i].getSensorType();
           } while (sensorityypit[i] == null || sensorityypit[i].length() == 0 ||
                   sensorityypit[i].charAt(0) == ' ');
           
           int viimeinen = sensorityypit[i].length() - 1;
           char merkki = sensorityypit[i].charAt(viimeinen);
           jakajat[i] = merkki == 'L' ? 2 : merkki == 'M' ? 1 : 0;
           System.out.println("Sensorin tyyppi on " + jakajat[i]);
       }
       Button.waitForPress(3000);
   }

   public void alusta() {
       moottori.setSpeed(720); // Full throttle!
       asetaKulma(0, false);
   }

   public void asetaKulma(int kulma, boolean odotus) {
       moottori.rotateTo(kulma, odotus); //false odotetaan, true ei odoteta
   }

   public int[] haeArvot(int määrä) {
       int[] sensorikulmat = laskeSensoriKulmat(määrä);
       int[] arvot = new int[määrä];
       int alkuKulma = moottori.getTachoCount();
       int kulma;
       
       for (int i = 0, j = määrä/2; j < määrä; ++i, ++j) {
           kulma = sensorikulmat[i] - alkuKulma;
           asetaKulma(kulma, false);
           for (int k = 0; k < 2; ++k) {
              int index = i+k*määrä/2;
              if (jakajat[k] == 0)
                  arvot[index] = sensorit[k].getDistLSB();
              else if (jakajat[k] == 1)
                  arvot[index] = sensorit[k].getDistLSB();
              else if (jakajat[k] == 2)
                  arvot[index] = sensorit[k].getDistLSB();
            
              // Pakota data aopivalle välille. Jos koneeseen vahingossa tuli
              // pitkän matkan sensori, niin hylkää ylipitkät tulokset. Jos
              // taas mittaustulos on liian lähellä sensoria, niin tulos ei ole
              // luotettava ja se voidaan vetäistä lähemmäs nollaa.
              //if (arvot[index] > 800) arvot[index] = 800;
              //else if (arvot[index] > 200) arvot[index] /= 2;
           }
       }

       asetaKulma(0, true);
       return arvot;
   }

    private int[] laskeSensoriKulmat(int määrä) {
        
        if (määrä < 2) {
            int[] kulmat = { -90 };
            return kulmat;
        }

        int[] sensorikulmat = new int[määrä];
        double kulma = -90.0;
        double askel = 180.0 / (määrä - 1);
        for (int i = 0; i < määrä; i++) {
            sensorikulmat[i] = (int) (kulma + 0.5);
            kulma += askel;
        }
        
        return sensorikulmat;
    }
}
